/*******************************************************************************
* FILE NAME: user_routines_sensor_test.c <VEX VERSION>
*
* DESCRIPTION:
*  This file contains the code to handle a light sensor, line follower, encoder,
*  and a Sonar Device.   
*
* ASSUMPTIONS:
*   A light sensor should be connected to Digital pin 1
*   A line following sensor should be connected to Digital pin 2
*     The surface must be black and the sensor must be at least 1 inch away from
*     it.  The white line must be at least 3/4" wide for proper detection
*     
*   Encoder not ready yet
*   The Sonar pulse input line (trigger pulse) should be connected to rc_dig_out04
*     It's output line (echo pulse) should be connected to rc_dig_int1
*
* USAGE:
*  You can either modify this file to fit your needs, or remove it from your 
*  project and replace it with a modified copy. 
*
*******************************************************************************/

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "printf_lib.h"

#define CODE_VERSION            3 /* Use this def. to keep track of your version # */

#define NEUTRAL_VALUE   118

unsigned char triggerCnt;         /* Used to trigger a 13us pulse to Sonar every 50ms */

unsigned char myval;              /* Used to create a 13us pulse */
unsigned char switchCounter;      /* Used to create a switch toggle */
unsigned char switchToggle;       /* Value toggles if the switch is pressed */
extern unsigned char fltHiResCounter; /* Filtered data from Sonar Device */
extern unsigned char fltLowResCounter;
extern unsigned char SonarDataValid;   /* Data, from the Sonar, is ready for processing */
extern unsigned char Delta;       /* Sonar data difference from last data read */
bitid oneShotFlags;               /* Used to display data one time per change */

void Handle_Sensors(void);

static void Setup_Who_Controls_Pwms(int pwmSpec1,int pwmSpec2,int pwmSpec3,int pwmSpec4,
                                    int pwmSpec5,int pwmSpec6,int pwmSpec7,int pwmSpec8)
{
  txdata.pwm_mask = 0xFF;         /* Default to master controlling all PWMs. */
  if (pwmSpec1 == USER)           /* If User controls PWM1 then clear bit0. */
    txdata.pwm_mask &= 0xFE;      /* same as txdata.pwm_mask = txdata.pwm_mask & 0xFE; */
  if (pwmSpec2 == USER)           /* If User controls PWM2 then clear bit1. */
    txdata.pwm_mask &= 0xFD;
  if (pwmSpec3 == USER)           /* If User controls PWM3 then clear bit2. */
    txdata.pwm_mask &= 0xFB;
  if (pwmSpec4 == USER)           /* If User controls PWM4 then clear bit3. */
    txdata.pwm_mask &= 0xF7;
  if (pwmSpec5 == USER)           /* If User controls PWM5 then clear bit4. */
    txdata.pwm_mask &= 0xEF;
  if (pwmSpec6 == USER)           /* If User controls PWM6 then clear bit5. */
    txdata.pwm_mask &= 0xDF;
  if (pwmSpec7 == USER)           /* If User controls PWM7 then clear bit6. */
    txdata.pwm_mask &= 0xBF;
  if (pwmSpec8 == USER)           /* If User controls PWM8 then clear bit7. */
    txdata.pwm_mask &= 0x7F;
}

void User_Initialization (void)
{
  IO1 = IO2 = IO3 = INPUT;        /* Used for analog inputs. */

  Set_Number_of_Analog_Channels(THREE_ANALOG);     /* See ifi_aliases.h */

  IO6 = IO8 = IO10 = INPUT;     
  IO12 = INPUT;        

  IO4 = IO5 = IO7 = IO9 = OUTPUT;     
  IO11 = IO13 = IO14 = IO15 = IO16 = OUTPUT;   

  rc_dig_out04 = rc_dig_out05 = rc_dig_out07 = 0;
  rc_dig_out09 = rc_dig_out11 = 0;
  rc_dig_out13 = rc_dig_out14 = rc_dig_out15 = rc_dig_out16 = 0;

  pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;

  Setup_Who_Controls_Pwms(MASTER,MASTER,MASTER,MASTER,MASTER,MASTER,MASTER,MASTER);
  Setup_PWM_Output_Type(IFI_PWM,IFI_PWM,IFI_PWM,IFI_PWM);

  T0CON = 0;                /* Timer 0 will be used for the sonar sensor */
  T0CONbits.T08BIT = 1;     /* Make it an 8 bit timer */
  T0CONbits.T0PS0 = 0;
  T0CONbits.T0PS1 = 0;
  T0CONbits.T0PS2 = 0;
  T0CONbits.TMR0ON = 1;     /* Turn it on */
  myval = 0xC0;             /* Yeilds a 13us delay */
  INTCONbits.TMR0IE = 0;    /* disable TMR0 ints for now */
  INTCON2bits.TMR0IP = 0;   /* Make TMR0 low priority */
  INTCON3bits.INT2IE = 0;   /* disable rc_dig_int1 for now */

  T2CON = 0;                /* Timer 2 will be used for the shaft encoder sensor */
  T2CONbits.T2CKPS1 = 1;    /* Prescaler is 16 */
  T2CONbits.T2OUTPS0 = 1;   /* Postscale is 16 */
  T2CONbits.T2OUTPS1 = 1;
  T2CONbits.T2OUTPS2 = 1;
  T2CONbits.T2OUTPS3 = 1;
  T2CONbits.TMR2ON = 1;     /* Turn Timer 2 on */
  PIE1bits.TMR2IE = 0;      /* disable TMR2 ints for now */
  INTCON2bits.INTEDG3 = 0;  /* set rc_dig_int2 falling edge triggered */
  INTCON2bits.INT3IP = 0;   /* set rc_dig_int2 low priority */
  INTCON3bits.INT3IF = 0;   /* clear int flag for rc_dig_int2 */
  INTCON3bits.INT3IE = 1;   /* enable rc_dig_int2 */

  Initialize_Serial_Comms();   
  
  txdata.user_cmd = 0x02;       /* Tell master you want to be in auton mode (Receivers disabled) */
  switchToggle = 0;             /* Turn toggle switch flag off */

  Putdata(&txdata);             /* DO NOT CHANGE! */
  User_Proc_Is_Ready();         /* DO NOT CHANGE! - last line of User_Initialization */

#ifdef _SIMULATOR
  statusflag.NEW_SPI_DATA = 1;
#else
  /* This code receives the 1st packet from master to obtain the version # */
  while (!statusflag.NEW_SPI_DATA);  /* Wait for 1st packet from master */
  Getdata(&rxdata);   
  printf("VEX - Master v%d, User v%d\n",(int)rxdata.master_version,(int)CODE_VERSION);        
#endif
}

void Debounce_Switch(void)
{
  if (switchCounter == 0)     
  {
    if (rc_dig_int3 == CLOSED)
    {
      switchToggle ^= 1;  /* debounce every 500ms if switch is active */
      switchCounter = 27;
    }
    else
    {
      switchCounter = 5;  /* since switch is inactive debounce faster */
    }
  }
  else
  {
    switchCounter--;
  }
}

void Handle_Sensors(void)
{
  static unsigned int LightSensor,LineFollower,Pot;

  Debounce_Switch();

  /* Read data from the light sensor */
  LightSensor = Get_Analog_Value(rc_ana_in01);

  if (LightSensor < 0x20)
  {
    if (!oneShotFlags.bit0) 
    {
      printf("Bright light detected\n");
      oneShotFlags.bit0 = 1;
      oneShotFlags.bit1 = 0;
      oneShotFlags.bit2 = 0;
      rc_dig_out13 = 1;
    }
  }
  else if (LightSensor < 0x60)
  {
    if (!oneShotFlags.bit1)
    {
      printf("Dim light detected\n");
      oneShotFlags.bit1 = 1;
      oneShotFlags.bit0 = 0;
      oneShotFlags.bit2 = 0;
    }
  }
  else if (!oneShotFlags.bit2)
  {
    printf("Dark light detected\n");
    oneShotFlags.bit2 = 1;
    oneShotFlags.bit0 = 0;
    oneShotFlags.bit1 = 0;
    rc_dig_out13 = 0;
  }

  /* Read data from the line follower sensor - Make sure it's at least 1 inch away from the surface */
  LineFollower = Get_Analog_Value(rc_ana_in02);

  if (LineFollower < 0x30)
  {
    if (!oneShotFlags.bit3) 
    {
      printf("White line detected\n");
      oneShotFlags.bit3 = 1;
      oneShotFlags.bit4 = 0;
      rc_dig_out14 = 1;
    }
  }
  else if (LineFollower > 0x200)
  {
    if (!oneShotFlags.bit4)
    {
      printf("Black surface detected\n");
      oneShotFlags.bit4 = 1;
      oneShotFlags.bit3 = 0;
      rc_dig_out14 = 0;
    }
  }

  if (switchToggle)
  {
    /* Read Pot to adjust motor speed (pwm01) */
    Pot = Get_Analog_Value(rc_ana_in03);
    pwm01 = (unsigned char) (Pot >> 2);
  }
  else
    pwm01 = 127;






  if (SonarDataValid){  //If Sonar data is valid drive the robot until it comes close to a wall
    SonarDataValid = 0;  //invalidate the sonar reading (reset)
    if (rc_dig_int6 == CLOSED)  
      printf("H %d, L %d, D %d\n",(int)fltHiResCounter,(int)fltLowResCounter,(int)Delta);  
    if ( (fltHiResCounter >= 16) && (fltHiResCounter <= 19) )
      rc_dig_out15 = 1;
    else
      rc_dig_out15 = 0;
  }

  else{
    if (rc_dig_int5 == CLOSED)
      printf("light val %x, line val %x pot val %x\n",LightSensor,LineFollower,(int)pwm01);
  }

  triggerCnt++;
  if (triggerCnt > 2)         /* expires every 4 * 18.5ms */
  {
    INTCON2bits.INTEDG2 = 1;  /* rc_dig_int1 - Interrupt on rising edge */
    INTCONbits.TMR0IE = 0;    /* make sure TMR0 Int is off */
    T0CONbits.T0PS0 = 0;
    TMR0H = 0;
    INTCONbits.TMR0IF = 0;
    TMR0L = myval;
    rc_dig_out04 = 1;         /* prepare a 13us pulse - rising edge of pulse */
    while (!INTCONbits.TMR0IF);
    rc_dig_out04 = 0;          /* falling edge of pulse */
    triggerCnt = 0;           /* reset trigger counter */
    INTCON3bits.INT2IE = 1;   /* enable rc_dig_int1*/
  }
}

void Process_Data_From_Master_uP(void)
{
  static unsigned int LightSensor,LineFollower;

  Getdata(&rxdata);   /* Get fresh data from the master microprocessor. */

  Handle_Sensors();

  Putdata(&txdata);             /* DO NOT CHANGE! */
}


